/**
  ******************************************************************************
  * File Name          : main.c
  * Description        : Main program body
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f1xx_hal.h"
#include "cmsis_os.h"
#include "usb_device.h"

/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "rplidar.h"

/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim7;

UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
UART_HandleTypeDef huart3;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart2_rx;
DMA_HandleTypeDef hdma_usart2_tx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart3_tx;

osThreadId lidarTaskHandle;

/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM2_Init(void);
static void MX_TIM7_Init(void);
static void MX_USART3_UART_Init(void);
void StartDefaultTask(void const * argument);                                    
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
                                

/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/

#ifdef __GNUC__
/* With GCC/RAISONANCE, small printf (option LD Linker->Libraries->Small printf
   set to 'Yes') calls __io_putchar() */
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif /* __GNUC__ */

/**
  * @brief  Retargets the C library printf function to the USART.
  * @param  None
  * @retval None
  */
PUTCHAR_PROTOTYPE
{
    /* Place your implementation of fputc here */
    /* e.g. write a character to the USART */
    HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1,10);
    return ch;
}
/* USER CODE END PFP */

/* USER CODE BEGIN 0 */
#define FREQ 4499
#define motor_dutyCycle FREQ*0.60
#define PI 3.141592

// Rx_Buffer[2000], USART_Transfer_cplt=0;
//uint32_t Rx_indx=0;
unsigned char Rx_indx2, Rx_data2, Rx_Buffer2[100], USART_Transfer_cplt2=0;

//char EAI_Start_Byte=0xa5;
//char EAI_Start_Scan=0x20;
//char EAI_Stop_Scan=0x25;
//char EAI_ForceStart=0x21;
//char EAI_GetHealth=0x52;
//char EAI_Reset=0x40;

//extern uint8_t ans_data[];
//extern uint16_t scan_loop_num;
//extern uint8_t data_scan_flag;
//uint8_t Lidar_receive_time=10;
//uint8_t flag1=1;
//int num;
//int16_t obstacle[5][361];

Lidar_Type lidar1;

/* USER CODE END 0 */

int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_TIM2_Init();
  MX_TIM7_Init();
  MX_USART3_UART_Init();

  /* USER CODE BEGIN 2 */
	  UART2_Printf("UART2_Printf start before lidar receive\r\n");
//		printf("printf start before lidar receive\r\n");
   Start_Receive_lidardata(1);
	 HAL_UART_Receive_DMA(&huart2,&Rx_data2,1);
	 HAL_TIM_Base_Start_IT(&htim7);//start timer 7 interrupt
	 
	 HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
   __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, motor_dutyCycle);	
  UART2_Printf("start\r\n");
	RPLIDAR_SCAN_START(1,&lidar1);

	 //Initial the obstacle 
//	 for(uint8_t i=0;i<5;i++)
//	   for(uint16_t m=0;m<361;m++)
//		    obstacle[i][m]=0x0000;

	 //printf("Hello World\r\n");
//	 printf("%c",EAI_Start_Byte);
//   printf("%c",EAI_Start_Scan);
	 


  /* USER CODE END 2 */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the thread(s) */
  /* definition and creation of lidarTask */
  osThreadDef(lidarTask, StartDefaultTask, osPriorityNormal, 0, 128);
  lidarTaskHandle = osThreadCreate(osThread(lidarTask), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */
 

  /* Start scheduler */
  osKernelStart();
  
  /* We should never get here as control is now taken by the scheduler */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */

  while (1)
  {
  /* USER CODE END WHILE */

  /* USER CODE BEGIN 3 */


  }
  /* USER CODE END 3 */

}

/** System Clock Configuration
*/
void SystemClock_Config(void)
{

  RCC_OscInitTypeDef RCC_OscInitStruct;
  RCC_ClkInitTypeDef RCC_ClkInitStruct;
  RCC_PeriphCLKInitTypeDef PeriphClkInit;

    /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
  PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Configure the Systick interrupt time 
    */
  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);

    /**Configure the Systick 
    */
  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}

/* TIM2 init function */
static void MX_TIM2_Init(void)
{

  TIM_MasterConfigTypeDef sMasterConfig;
  TIM_OC_InitTypeDef sConfigOC;

  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 4499;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  HAL_TIM_MspPostInit(&htim2);

}

/* TIM7 init function */
static void MX_TIM7_Init(void)
{

  TIM_MasterConfigTypeDef sMasterConfig;

  htim7.Instance = TIM7;
  htim7.Init.Prescaler = 119;
  htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim7.Init.Period = 59999;
  if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART1 init function */
static void MX_USART1_UART_Init(void)
{

  huart1.Instance = USART1;
  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART2 init function */
static void MX_USART2_UART_Init(void)
{

  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART3 init function */
static void MX_USART3_UART_Init(void)
{

  huart3.Instance = USART3;
  huart3.Init.BaudRate = 115200;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart3) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/** 
  * Enable DMA controller clock
  */
static void MX_DMA_Init(void) 
{
  /* DMA controller clock enable */
  __HAL_RCC_DMA1_CLK_ENABLE();

  /* DMA interrupt init */
  /* DMA1_Channel2_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
  /* DMA1_Channel3_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
  /* DMA1_Channel4_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
  /* DMA1_Channel5_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
  /* DMA1_Channel6_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn);
  /* DMA1_Channel7_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn);

}

/** Configure pins as 
        * Analog 
        * Input 
        * Output
        * EVENT_OUT
        * EXTI
*/
static void MX_GPIO_Init(void)
{

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

}

/* USER CODE BEGIN 4 */
static char uart2_tx_buffer[128];
uint8_t BUFFER_LENGTH=127;

void UART2_Printf(const char* format,...)
{
    int i=-1;

    //uint8_t result;


    va_list arg;
    va_start(arg,format);
    i=vsnprintf((char *)&uart2_tx_buffer,BUFFER_LENGTH,format,arg);
	//  i=vsprintf((char *)&uart2_tx_buffer,format,arg);


    if(i>0)
    {
			uint8_t a=0;
			while((a<BUFFER_LENGTH)&&uart2_tx_buffer[a])
			{
				 HAL_UART_Transmit(&huart2, (uint8_t *)&uart2_tx_buffer[a++], 1,10);
				// a++;

			}
		//	 HAL_UART_Transmit(&huart2, (uint8_t *)&uart2_tx_buffer, i, 1);

    }
    va_end(arg);
}

/* USER CODE END 4 */

/* StartDefaultTask function */
void StartDefaultTask(void const * argument)
{
  /* init code for USB_DEVICE */
  MX_USB_DEVICE_Init();

  /* USER CODE BEGIN 5 */
  /* Infinite loop */
  for(;;)
  {
//		printf("Hello usart1\r\n");
//		UART2_Printf("Hello USART2\r\n");
      if(USART_Transfer_cplt2)
		{
			
				if(Rx_Buffer2[0]=='r')
				{
//				printf("%c",EAI_Start_Byte);
//				printf("%c",EAI_Start_Scan);
					RPLIDAR_SCAN_START(1,&lidar1);
				}
				else if(Rx_Buffer2[0]=='s')
				{
					RPLIDAR_SCAN_Stop(1,&lidar1);
				}
//				else if(Rx_Buffer2[0]=='m')
//				{
//						
////					printf("%c%c",EAI_Start_Byte,0x50);
//			//		printf("%c",0x50);
//				}
//				else if(Rx_Buffer2[0]=='f')
//				{
////					printf("%c",EAI_Start_Byte);
////					printf("%c",EAI_ForceStart);
//				}
//				else if(Rx_Buffer2[0]=='h')
//				{
////					printf("%c",EAI_Start_Byte);
////					printf("%c",EAI_GetHealth);
//				}
				else 
					printf("Wrong Message");
				
			USART_Transfer_cplt2=0;
//			memset(Rx_Buffer2,0,sizeof(Rx_Buffer2));
			for (int i=0; i<100; i++)
			{
					Rx_Buffer2[i] = 0;
			}

		}
//		if(isReceiveOvertime(1)==1)
//		{
//			UART2_Printf("Receive over time\r\n");
//			RPLIDAR_RESTARTSCAN(1,&lidar1);
//		//	collect_num=0;
//		
//		}
//		else
//		{
			if(collect_LIDARdata(1,&lidar1)==1)
			{
				 for(uint16_t angle=0;angle<90;angle++)
					if(Detect_Obstacles(&lidar1,0,200,600,1000,angle)==1)
						UART2_Printf("There is a obtacle: %d Distance: %d\r\n",angle,Get_Distance(&lidar1,angle));
			}
			else
			  UART2_Printf("***\r\n");

			Get_ScanData(1);

				
	//	} 
//      if(data_scan_flag==1)
//      {
//				  if(flag1==1)
//					{
//		        for(uint8_t i=0;i<7;i++)
//           {
//              if(ans_data[i]==0x00)
//                  UART2_Printf("%02x",0x00);
//              else
//                  UART2_Printf("%02x",ans_data[i]);        
//								
//							ans_data[i]=0x00;
//           }
//						flag1=0;
//					}


//				if(Lidar_receive_time==0)
//				{
//					UART2_Printf("Receive Over time\r\n");
//					printf("%c",EAI_Start_Byte);
//					printf("%c",EAI_Stop_Scan );
//					osDelay(500);
//					for (int i=0; i<Rx_indx; i++)
//					{
//							Rx_Buffer[i] = 0x00;
//					}
//					Rx_indx=0;
//					data_scan_flag=0;
//					USART_Transfer_cplt=0;
//					flag1=1;
//					
//					printf("%c",EAI_Start_Byte);
//					printf("%c",EAI_Start_Scan );
//					
//				}
//				

//					//flag1=0; 
//					
//      
//      }


     // if(scan_loop_num==2&&flag)
//			  if(USART_Transfer_cplt)
//				{
					//UART2_Printf("USART_Transfer_cplt");
//						for(uint16_t i=0;i<Rx_indx;i+=5)
//						{
//							uint16_t angle=(uint16_t)(Rx_Buffer[i+2]<<1)+((float)Rx_Buffer[i+1]/128+0.55);
//							uint16_t distance=(uint16_t)(Rx_Buffer[i+4]<<6)+(Rx_Buffer[i+3]>>2);
////							uint8_t zone=angle/90;
////							int16_t x=0,y=0;
////							double radian_angle;
//							//if(distance>0)
//								obstacle[num][angle]=distance;
//						}
//								//UART2_Printf("BEFORE COMPUTATE : Angle : %d; Distance : %d\r\n",angle,distance);
//						for(uint16_t i=0;i<361;i++)
//						{
//							uint8_t zone=i/90;
//							int16_t x=0,y=0;
//							double radian_angle;
//							uint8_t effectiveNum=0;
//							uint16_t distance=0;
//								for(uint8_t m=0;m<5;m++)
//								  if(obstacle[m][i]>0)
//									{
//										distance+=obstacle[m][i];
//										effectiveNum++;
//									
//									}
//									if(effectiveNum==0)
//										distance=0;
//									else
//							      distance/=effectiveNum;
//							if(distance>0)
//							switch (zone)
//							{
//								case 0:
//									radian_angle=(double)i*PI/180;
//									x=distance*sin(radian_angle);
//								  y=distance*cos(radian_angle);
//									if((x<200)&&(y<600))
//							    	UART2_Printf("Obstacle : Angle : %d; Distance : %d\r\n",i,distance);
//									break;
//								case 3:
//									radian_angle=((double)i-270.0)*PI/180;
//								  x=distance*cos(radian_angle)*(-1);
//								  y=distance*sin(radian_angle);
//									if((abs(x)<200)&&(y<600))
//    								UART2_Printf("Obstacle : Angle : %d; Distance : %d\r\n",i,distance);
//								  break;
//								default: 
//									break;

//							}

//							
//						 }
//						  num++;
//							if(num==5)
//							{
//								for(uint8_t i=0;i<4;i++)
//								for(uint16_t m=0;m<361;m++)
//								{
//									obstacle[i][m]=0x0000;
//								  obstacle[i][m]=obstacle[i+1][m];
//								
//								}
//								
//								for(uint16_t m=0;m<361;m++)
//									obstacle[4 ][m]=0x0000;
//								num--;

//							
//							}
//						


//						for (int i=0; i<Rx_indx; i++)
//						{
//								Rx_Buffer[i] = 0x00;
//						}
//						Rx_indx=0;
//				//	scan_loop_num=0;
//					USART_Transfer_cplt=0;
						
//				UART2_Printf("**\r\n");
//				}
      
    osDelay(5);
  }
	
  /* USER CODE END 5 */ 
}

/**
  * @brief  This function is executed in case of error occurrence.
  * @param  None
  * @retval None
  */
void _Error_Handler(char * file, int line)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  while(1) 
  {
  }
  /* USER CODE END Error_Handler_Debug */ 
}

#ifdef USE_FULL_ASSERT

/**
   * @brief Reports the name of the source file and the source line number
   * where the assert_param error has occurred.
   * @param file: pointer to the source file name
   * @param line: assert_param error line source number
   * @retval None
   */
void assert_failed(uint8_t* file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
    ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */

}

#endif

/**
  * @}
  */ 

/**
  * @}
*/ 

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
